/*!
 * Copyright (c) 2012
 * Automation and Robotics Lab (LARA) at University of Brasilia (UnB)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     - Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     - Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     - Neither the name of the Automation and Robotics Lab (LARA) nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 *****************************************************************
 *
 *   ROS stack name: schunk
 *   ROS package name: schunk_high
 *
 *   Authors: André A. Geraldes, email: andregeraldes@lara.unb.br
 *            Murilo M. Marinho, email: murilomarinho@lara.unb.br
 *
 *****************************************************************/

#ifndef ROSPKG_SCHUNK_HIGH_CARTESIAN_CONTROLLER_H_
#define ROSPKG_SCHUNK_HIGH_CARTESIAN_CONTROLLER_H_

/******************************************************************************
                                   INCLUDES
 *****************************************************************************/

// General
#include <vector>
#include <string>

// ROS
#include <ros/ros.h>
#include <ros/callback_queue.h>

// Schunk_high
#include <schunk_high/robot.h>

// Schunk_low
#include <schunk_low/schunk_low_control.h>

// Internal algebra
#include <schunk_high/gmatrix.h>
#include <schunk_high/gmatrix_plus.h>
#include <schunk_high/gmatrix_linalg.h>
#include <schunk_high/dualquaternion.h>

// Messages
#include <schunk_msgs/JointPositions.h>
#include <schunk_msgs/JointVelocities.h>

// Service Servers
#include <schunk_high/Init.h>
#include <schunk_high/Enable.h>
#include <schunk_high/Disable.h>

class CartesianController
{
    protected:

        // Controller name (For parameter server loading procedures)
        std::string controller_name_;

        // Node Handlers
        ros::NodeHandle nodehandle_;
        ros::NodeHandle service_server_nodehandle_;
        ros::NodeHandle service_client_nodehandle_;
        ros::NodeHandle publisher_nodehandle_;
        ros::NodeHandle references_nodehandle_;

        // Callback Queues
        ros::CallbackQueue service_server_queue_;
        ros::CallbackQueue service_client_queue_;
        ros::CallbackQueue publisher_queue_;
        ros::CallbackQueue references_queue_;

        // Publishers
        ros::Publisher publisher_velocity_;
        ros::Publisher publisher_position_;

        // Service Clients
        ros::ServiceClient service_client_init_;
        ros::ServiceClient service_client_stop_;
        ros::ServiceClient service_client_recover_;
        ros::ServiceClient service_client_connect_;
        ros::ServiceClient service_client_disconnect_;
        ros::ServiceClient service_client_control_mode_;
        ros::ServiceClient service_client_get_pos_vel_;

        // Messages    ros::ServiceClient service_client_get_pos_vel_;
        schunk_msgs::JointVelocities publisher_velocity_message_;
        schunk_msgs::JointPositions publisher_position_message_;

        // Robot object
        Robot* robot_;

        // Dual Quaternion Kinematics Calculation Variables
        // Jacobians
        PGMATRIX task_jacobian_;

        // PID errors.
        PGMATRIX kp_;
        PGMATRIX ki_;
        PGMATRIX kd_;

        // Thetas.
        PGMATRIX thetas_;
        PGMATRIX delta_thetas_;

        // Reference Signal.
        PGMATRIX reference_state_variables_;

        // Measured Values
        PGMATRIX measured_state_variables_;

        // Errors
        PGMATRIX error_;
        PGMATRIX last_error_;
        PGMATRIX integral_error_;

        // Auxiliar Matrices
        PGMATRIX aux_matrix_1_;
        PGMATRIX aux_matrix_2_;
        PGMATRIX aux_matrix_3_;

        DQ* end_effector_pose_;
        Q* translation_;

        // Robot initial position
        std::vector<double> initial_position_;

        // Loop Variables
        double sample_rate_;
        double saturation_velocity_;

        // Status variables
        bool initialized_;
        bool enabled_;
        bool robot_dh_loaded_;
        bool control_parameters_loaded_;
        bool initial_position_loaded_;
        bool move_to_initial_position_;

        unsigned int reference_size_;
        int controller_NID_;

        // Service Client Wrap Functions
        bool InitSchunk();
        bool StopSchunk();
        bool RecoverSchunk();
        bool ConnectToSchunk();
        bool DisconnectFromSchunk();
        bool ChangeSchunkControlMode(int mode);
        bool GetSchunkPositionsAndVelocitiesAndUpdateThetasAndDThetas();

        // Publisher Wrap Functions
        virtual void SetSchunkVelocitiesAndSend();
        void SendJointPositionsToSchunk(std::vector<double> positions);

        // Auxiliar Functions
        bool InitKinematicVariables(std::vector<double> KP, std::vector<double> KI, std::vector<double> KD);
        void ControlCleanUp();
        bool MoveToInitialPosition(double tolerance);

        // Virtual functions for the Services Server Callbacks
        virtual bool InitCallback(schunk_high::Init::Request& req, schunk_high::Init::Response& resp)=0;
        virtual bool EnableCallback(schunk_high::Enable::Request& req, schunk_high::Enable::Response& resp)=0;
        virtual bool DisableCallback(schunk_high::Disable::Request& req, schunk_high::Disable::Response& resp)=0;

    public:

        // Constructor
        CartesianController(std::string controller_name, int controller_NID, int reference_size);

        // Destructor
        ~CartesianController();

        // Parameter Load Functions
        bool GetRobotControlParameters();
        bool GetRobotDH();
        bool GetRobotInitialPosition();

        // Virtual function for the Control Loop
        virtual void Control()=0;
};

#endif  // ROSPKG_SCHUNK_HIGH_CARTESIAN_CONTROLLER_H_
